#include <stdio.h>
#include "SeqList.h"
#include "linkedlist.h"
#include "staticlist.h"
#include "sclinkedlist.h"
#include "DLinkedList.h"
#include "DCLinkedList.h"
#include "SeqStack.h"
#include "LinkStack.h"
#include "SeqQueue.h"
#include "LinkQueue.h"
#include "RQueue.h"
#include "SString.h"
#include "Sort.h"
#include "stdint.h"
#include "math.h"


#define CONTENT_LEN 80

#undef NULL
#define NULL 0
#define     BIT(n)              (1U << (n))
#define DEBUG
#ifdef DEBUG
#define printfS     printf
#define printfB8    printf_buff_byte
#define printfB32   printf_buff_word
#else
#define	printfS(format, ...)	     ((void)0)
#define	printfB8(buff, byte_len)	 ((void)0)
#define	printfB32(buff, word_len)	 ((void)0)
#endif
#define	ADJUST_SOUND_MAX	10

#define Fclk 48000000    // 主时钟频率 48 MHz
#define Prescaler 48    // 分频器值为 48

#include "kalman.h"


uint32_t outputUint32(uint8_t *buffPtr)
{
    uint32_t temp;

    temp = (((uint32_t)buffPtr[0]) << 24) + (((uint32_t)buffPtr[1]) << 16)
           + (((uint32_t)buffPtr[2]) << 8) + ((uint32_t)buffPtr[3]);

    return temp;
}

void inputUint32(uint8_t *buffPtr, uint32_t input)
{
    buffPtr[0] = (uint8_t)(input >> 24);
    buffPtr[1] = (uint8_t)(input >> 16);
    buffPtr[2] = (uint8_t)(input >> 8);
    buffPtr[3] = (uint8_t)(input);
}



void Calculate_PWM_Settings(uint32_t CCR)
{
    uint32_t ARR;
    float DutyCycle;
    float PWMFreq_Hz;
    float T_pwm;

    // 假设占空比为 50%（可以调整以匹配实际情况）
    uint16_t Wide_Thousandths = 500;

    // 计算 ARR
    ARR = (CCR * 1000.0 / Wide_Thousandths) - 1;

    // 计算 PWM 周期 (T_pwm)
    T_pwm = (ARR + 1) / (float)(Fclk / Prescaler);

    // 计算 PWM 频率
    PWMFreq_Hz = 1 / T_pwm;

    // 计算占空比
    DutyCycle = (CCR / (float)(ARR + 1)) * 1000;

    printf("ARR = %u\n", ARR);
    printf("PWM Frequency = %.2f Hz\n", PWMFreq_Hz);
    printf("Duty Cycle = %.2f%%\n", DutyCycle / 10.0);  // Wide_Thousandths / 1000 = 百分比
}

const uint16_t adjustSound[ADJUST_SOUND_MAX] =
        {
                0x0ACD, 0x09B8, 0x08A4, 0x078F, 0x067B,
                0x0566, 0x0452, 0x033D, 0x0229, 0x114,
        };
/**
 * 打印信息算法,只支持英文
 * @param ch
 */
static void print_content(char ch[CONTENT_LEN])
{
    printf("\n");
    for (int i = 0; i < CONTENT_LEN; i++)
    {
        printf("*");
    }
    printf("\n");

    int len = StrLength(ch);
    int pos = (CONTENT_LEN - 1) / 3;
    char chs[CONTENT_LEN];
    for (int i = 0; i < CONTENT_LEN; i++)
    {
        if (i == 0 || i == CONTENT_LEN - 1)
        {
            chs[i] = '*';
        } else
        {
            chs[i] = ' ';
        }
    }
    //替换
    StrDelete(&chs, pos, len);
    StrInsert(&chs, pos, ch);
    for (int i = 0; i < CONTENT_LEN; i++)
    {
        printf("%c", chs[i]);
    }
    printf("\n");
    for (int i = 0; i < CONTENT_LEN; i++)
    {
        printf("*");
    }
    printf("\n");
}

static void test(void){
    printf("test static \n");
}

#define GRID_LONG_LEVEL_1 6.0
#define GRID_LAT_LEVEL_1  4.0

/**
 * 转换
 * @param degree
 * @param minutes
 * @param seconds
 * @return
 */
double dms_to_decimal(int degree,int minutes,double seconds){
    return degree+minutes/60.0+seconds/3600.0;
}

/**
 * 纬度
 * @param latitude
 * @return
 */
char get_latitude_grid(double latitude){
    int lat_grid_num= (int)(ceil(fabs(latitude)/GRID_LAT_LEVEL_1));
    return 'A'+(lat_grid_num-1);
}

/**
 * 计算第一级网络编号（经度)
 * @param longitude
 * @return
 */
int get_longitude_grid(double longitude){
    int lon_grid_num;
    if(longitude>=0){
        //东经
        lon_grid_num=30+(int)ceil(longitude/GRID_LONG_LEVEL_1);
    }
    else{
        //西经
        lon_grid_num=30-(int)ceil(fabs(longitude)/GRID_LONG_LEVEL_1);
    }
    return lon_grid_num;
}

/**
 * 计算左下角经度
 * @param longitude
 * @return
 */
int get_longitude_grid_start(double longitude){
    //从1 开始计算
    return (get_longitude_grid(longitude)-(30+1))*GRID_LONG_LEVEL_1; //从1开始计算
}

/**
 * 计算左下角纬度
 * @param latitude
 * @return
 */
int get_latitude_grid_start(double latitude){
    int grid_num=(int)(ceil(fabs(latitude)/4.0));
    return (grid_num*4.0-4.0)*(latitude>=0?1:-1);  //从1开始计算
}
/**
 * 计算第二级网络
 */
int get_latitude_grid_level2(double latitude) {

}

int get_longitude_grid_level2(double longitude) {

}

/**
 * 生成编码
 * @param latitude
 * @param longitude
 * @param grid_code
 */
void cal_grid_code(double latitude,double longitude,char* grid_code){
    grid_code[0]=(latitude>=0)?'N':'S';
    int lon_grid= get_longitude_grid(longitude);
    snprintf(&grid_code[1],3,"%02d",lon_grid);
    grid_code[3]= get_latitude_grid(latitude);
    //结束
    grid_code[4]='\0';
}


void cal_grid(void){
    // 纬度 39°59'35.38"N 转换为十进制
    double latitude = dms_to_decimal(39, 59, 35.38);
    // 经度 116°18'45.37"E 转换为十进制
    double longitude = dms_to_decimal(116, 18, 45.37);

    char grid_grid[5];
    cal_grid_code(latitude,longitude,grid_grid);
    printf("result: %s \n",grid_grid);
    //第二级 获取角点经度
    int lon_start= get_longitude_grid_start(longitude);
    printf("long_start:%d \n",lon_start);
    printf("lat_start:%d \n", get_latitude_grid_start(latitude));
}

/**
 * 测试
 * @return
 * @return
 */
int main(void)
{
#if 0
    print_content("sequential table test");
    //顺序表
    SeqList mylist;
    SeqInitList(&mylist);

    ElemType Item[] = {1, 2, 3, 4, 5};
    //printf("%d \n",len(Item));
    for (int i = 0; i < len(Item); i++)
    {
        //push_back(&mylist,Item[i]);
        //push_front(&mylist,item[i]);
        seq_push_front(&mylist, Item[i]);
    }

    seq_show_list(&mylist);
    printf("deleted after data:");
    seq_pop_back(&mylist);
    seq_show_list(&mylist);
    seq_pop_front(&mylist);
    seq_show_list(&mylist);
    printf("array's length %d", seq_length(&mylist));
    seq_destroy(&mylist);
    seq_show_list(&mylist);
    //printf("array's length %d", len(&mylist));

    print_content("LinkedList test");
    List linkedlist;
    LinkedInitList(&linkedlist);
    ElemType item1[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
    Node *p = NULL;
    for (int i = 0; i < len(item1); i++)
    {
        //linked_push_back(&linkedlist,item1[i]);
        linked_push_front(&linkedlist, item1[i]);
    }
    linked_show_list(&linkedlist);
    //尾删除
    linked_pop_back(&linkedlist);
    linked_show_list(&linkedlist);
    //头删除
    linked_pop_front(&linkedlist);
    linked_show_list(&linkedlist);
    linked_insert_val(&linkedlist, 3);
    linked_show_list(&linkedlist);
    Node *n = linked_find(&linkedlist, 3);
    printf("%d-%d \n", n->data, n->next);

    //todo window运行有问题 static linked list test
//    printf("--------------static linked list test----------------------------\n");
//    StaticList sl;
//    InitStaticList(&sl);
//    for(int i=0;i<5;++i){
//        StaticList_insert(&sl,i);
//
//    }
//    StaticList_show(&sl);
//    StaticList_delete(&sl);
//    StaticList_show(&sl);

    print_content("loop linked lists");

    List sclinkedlist;
    InitScLinkedList(&sclinkedlist);
    ElemType itemScLinkedList[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
    Node *psc = NULL;
    for (int i = 0; i < len(itemScLinkedList); i++)
    {
        //scLinkedList_push_back(&sclinkedlist,itemScLinkedList[i]);
        scLinkedList_push_front(&sclinkedlist, itemScLinkedList[i]);
    }

    scLinkedList_insert_val(&sclinkedlist, 11);
    scLinkedList_insert_val(&sclinkedlist, 5);
    scLinkedList_show_list(&sclinkedlist);
    scLinkedList_pop_back(&sclinkedlist);
    scLinkedList_show_list(&sclinkedlist);
    scLinkedList_sort(&sclinkedlist);
    scLinkedList_show_list(&sclinkedlist);
    scLinkedList_resver(&sclinkedlist);
    scLinkedList_show_list(&sclinkedlist);

    print_content("Doubly linked lists");

    DList doubleLinkedlist;
    ElemType DLinkedListItem[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
    InitDList(&doubleLinkedlist);
//    for(int i=0;i< len(DLinkedListItem);++i){
//        DLinkedList_push_back(&doubleLinkedlist,DLinkedListItem[i]);
//    }
//
    for (int i = 0; i < len(DLinkedListItem); ++i)
    {
        //测试头插入法
        DLinkedList_push_front(&doubleLinkedlist, DLinkedListItem[i]);
    }
    DLinkedList_show_list(&doubleLinkedlist);

    print_content("Doubly SC linked lists");
    ElemType SCLinkedListItems[] = {11, 2, 33, 24, 5, 6, 70, 8, 90, 10};
    DList dcLinkedList;
    InitDCLinkedList(&dcLinkedList);
    for (int i = 0; i < len(SCLinkedListItems); ++i)
    {
        //DCLinkedList_push_back(&dcLinkedList,SCLinkedListItems[i]);
        DCLinkedList_push_front(&dcLinkedList, SCLinkedListItems[i]);
    }

    DCLinkedList_show_list(&dcLinkedList);
    DCLinkedList_pop_back(&dcLinkedList);
    DCLinkedList_show_list(&dcLinkedList);
    DCLinkedList_pop_front(&dcLinkedList);
    DCLinkedList_show_list(&dcLinkedList);
    DCLinkedList_sort(&dcLinkedList);
    DCLinkedList_show_list(&dcLinkedList);
    DCLinkedList_resver(&dcLinkedList);
    DCLinkedList_show_list(&dcLinkedList);

    print_content("SEQ STACK TEST");
    SeqStack st;
    InitSeqStack(&st);
    ElemType v;
    for (int i = 0; i <= 10; ++i)
    {
        SeqStack_Push(&st, i);
    }
    SeqStack_Show(&st);
    SeqStack_Pop(&st);
    SeqStack_Show(&st);
    if (SeqStack_GetTop(&st, &v))
    {
        printf("the element of stack top:%d \n", v);
    }
    SeqStack_Clear(&st);
    SeqStack_Show(&st);
    SeqStack_Destory(&st);

    print_content("LINK STACK TEST");
    LinkStack ls;
    InitLinkStack(&ls);

    for (int i = 1; i <= 5; ++i)
    {
        LinkStack_Push(&ls, i);
    }

    LinkStack_Pop(&ls);

    LinkStack_Show(&ls);
    printf("栈长度:%d \n", LinkStack_Length(&ls));
    ElemType v1;
    if (LinkStack_GetTop(&ls, &v1))
    {
        printf("栈顶元素:%d \n", v1);
    }
    LinkStack_Show(&ls);
    if (LinkStack_IsEmpty(&ls))
        printf("true \n");
    else
        printf("false \n");

    LinkStack_Destory(&ls);

    print_content("SEQ QUEUE TEST");
    Queue seqQueue;
    InitSeqQueue(&seqQueue);
    for (int i = 0; i < 8; ++i)
    {
        EnSeqQueue(&seqQueue, i);
    }
    SeqQueue_Show(&seqQueue);
    DeSeqQueue(&seqQueue);
    SeqQueue_Show(&seqQueue);
//    EnSeqQueue(&seqQueue,111);
//    SeqQueue_Show(&seqQueue);
    printf("%d \n", SeqQueue_Length(&seqQueue));

    print_content("R QUEUE TEST");
    Queue rq;
    InitRQueue(&rq);
    for (int i = 1; i <= 8; ++i)
    {
        EnRQueue(&rq, i);
    }
    RQueue_Show(&rq);
    printf("%d\n", RQueue_Length(&rq));
    ElemType v3;
    RQueue_Get_Header(&rq, &v3);
    printf("%d \n", v3);


    print_content("Link QUEUE TEST");
    LinkQueue lq;
    InitLinkQueue(&lq);
    for (int i = 1; i <= 10; i++)
    {
        EnLinkQueue(&lq, i);
    }
    LinkQueue_Show(&lq);
    DeLinkQueue(&lq);
    LinkQueue_Show(&lq);
    printf("Len=%d\n", LinkQueue_Length(&lq));


    print_content("sort test");

    int arr[] = {10, 8, 90, 21, 30, 39, 11, 5, 7, 100, 121};
    //SelectSort(&arr,10,0);
    //BulletSort(arr,10,0);
    InsertSort(arr, 10, 0);
    printf("选择排序后结果:");
    for (int i = 0; i < 10; ++i)
    {
        printf("%d ", arr[i]);
    }
    printf("\n");

    test();
    uint8_t i;
    uint16_t batVol=8389;
    uint8_t tempBuff[50];
    tempBuff[0] = 'A';
    tempBuff[1] = 'W';
    tempBuff[2] = ~'A';
    tempBuff[3] = ~'W';

    tempBuff[4] = 0x00;
    tempBuff[5] = (uint8_t) (batVol >> 8);
    tempBuff[6] = (uint8_t) batVol;

    for(i=0;i<7;i++){
        printf("%02x ",tempBuff[i]);
    }
    printf("\r\n");

    print_content("kalman");
    KalmanFilter kf;
    //过程噪声 测量噪声  初始化估计误差 初始化羚羊感冒片糖衣
    kalman_init(&kf, 0.01, 0.1, 1, 0);  // 初始化滤波器（假设初始状态为0）

    // 模拟一些测量数据（例如液位高度）
    float measurements[10] = {160, 1.1, 1.5, 1.3, 1.4, 1.6, 1.2, 1.3, 1.2, 1.0};

    for (int i = 0; i < 10; i++) {
        float estimate = kalman_update(&kf, measurements[i]);  // 更新滤波器并获取新的估计值
        printf("Measurement: %.2f, Estimate: %.2f\n", measurements[i], estimate);
    }
    int flg=1;
    if(!flg){
        printf("test \n");
    }

//    print_content("thread");
//
//    int arr_thread[5];
//    pthread_t thread[5];
//    int ret;
//    for(int i=0;i<5;i++){
//        arr_thread[i]=i;
//        ret= pthread_create(&thread[i],NULL,threadMethod,&arr_thread[i]);
//        if(ret!=0){
//            printf("pthread_create error, [%s]\n", strerror(ret));
//            return -1;
//        }
//
//    }
//    printf("main thread, pid is [%d], tid is [%ld]\n", getpid(), pthread_self());
//    sleep(30);
    //大端测试
    printf("\n");
    uint8_t buff[0];
    buff[0]=0x41;
    buff[1]=0x57;
    inputUint32(&buff[2], 110);
    inputUint32(&buff[6],110);

    for(int i=0;i<10;++i){
        printf( "%02x ",buff[i]);
    }

    printf("\n");
    cal_grid();


    int led1=1;
    int led2=0;

    int vol=8400;

    if(led1&&(!led2)&&vol<8400){
        printf("no full \r\n");
    }else{
        printf("full \r\n");
    }
#endif
    return 0;

}



/*
//扩展测试
void main()
{
    SeqList mylist,youlist,list;
    InitSeqList(&mylist);
    InitSeqList(&youlist);

    push_back(&mylist,1);
    push_back(&mylist,3);
    push_back(&mylist,5);
    push_back(&mylist,7);
    push_back(&mylist,9);

    push_back(&youlist,2);
    push_back(&youlist,4);
    //push_back(&youlist,6);
    push_back(&youlist,8);
    //push_back(&youlist,10);

    merge(&list,&mylist,&youlist);
    show_list(&list);

}
*/